#include "node_rts.hh"

NodeRts::NodeRts ()
  : NB_NODE_256_TO_8 (37449)
  , INDEX_8 (4681)
  , INDEX_16 (585)
  , INDEX_32 (73)
  , INDEX_64 (9)
  , INDEX_128 (1)
{
  dyn_value* tmp = 0;

  // on creer le tableau de noeuds
  static_tab_ = new static_node_obj[NB_NODE_256_TO_8];

  // on le rempli avec les valeurs de base
  for (int i = 0; i < NB_NODE_256_TO_8; ++i)
    {
      static_tab_[i].is_surface = 0;
      static_tab_[i].is_full_empty = 0;

      // pour les niveaux de 8 on genere un type particulier de noeud
      if (i >= INDEX_8)
	{
	  tmp = new dyn_value;
	  for (int j = 0; j < 8; ++j)
	    tmp->is_full_empty[j] = 0;
	  tmp->obj_list = 0;

	  static_tab_[i].obj_list = (List<Object*>*) tmp;
	}
      else
	static_tab_[i].obj_list = 0;
    }
}

NodeRts::~NodeRts ()
{
  List<Object*>* tmp = 0;
  List<Object*>* pred = 0;

  List<dynamic_node*>* d_tmp = 0;
  List<dynamic_node*>* d_pred = 0;

  // pour tous les noeuds
  for (int i = 0; i < NB_NODE_256_TO_8; ++i)
    {
      tmp = static_tab_[i].obj_list;
      // si on est sur un noeud de 8 ...
      if (i >= INDEX_8)
	{
	  d_tmp = ((dyn_value*) tmp)->obj_list;
	  while (d_tmp != 0) // ... on parcours la liste d'objets et on les detruits
	    {
	      d_pred = d_tmp;
	      if (d_pred->elt != 0)
		delete d_pred->elt;
	      d_tmp = d_tmp->next;
	      delete d_pred;
	    }
	  delete tmp; // et on detruit le pointeur sur le noeud special
	}
      else // si on est sur un noeud >= 16 ...
	while (tmp != 0)
	  {
	    // on detruit juste la liste des noeuds
	    pred = tmp;
	    if (pred->elt != 0)
	      delete pred->elt;
	    tmp = tmp->next;
	    delete pred;
	  }
    }

  // et on finit par detruire le tableau de noeud
  delete[] static_tab_;
}

void
NodeRts::add_obj (Object* obj)
{
  Vector3f pos = obj->pos_get ();
  pos %= 256; // recadrer dans le carre 256 * 256 * 256
  Vector3f size = obj->size_get ();

  Vector3d block_id (((int) pos (0)) / 16,
		     ((int) pos (1)) / 16,
		     ((int) pos (2)) / 16); // id du block en 3D

  Vector3d front_id (((int) pos (0) + (int) size (0)) / 16,
		     ((int) pos (1)) / 16,
		     ((int) pos (2)) / 16); // id du block en 3D

  Vector3d back_id (((int) pos (0) - (int) size (0)) / 16,
		    ((int) pos (1)) / 16,
		    ((int) pos (2)) / 16); // id du block en 3D

  Vector3d left_id (((int) pos (0)) / 16,
		    ((int) pos (1) - (int) size (1)) / 16,
		    ((int) pos (2)) / 16); // id du block en 3D

  Vector3d right_id (((int) pos (0)) / 16,
		     ((int) pos (1) + (int) size (1)) / 16,
		     ((int) pos (2)) / 16); // id du block en 3D

  Vector3d top_id (((int) pos (0)) / 16,
		   ((int) pos (1)) / 16,
		   ((int) pos (2) + (int) size (2)) / 16); // id du block en 3D

  Vector3d bottom_id (((int) pos (0)) / 16,
		      ((int) pos (1)) / 16,
		      ((int) pos (2) - (int) size (2)) / 16); // id du block en 3D

  int i = INDEX_16;
  int j = i - INDEX_32;

  // tant que je ne suis pas entierement contenu dans un bloc
  while (front_id != block_id ||
	 back_id != block_id ||
	 right_id != block_id ||
	 left_id != block_id ||
	 top_id != block_id ||
	 bottom_id != block_id)
    {
      front_id /= 2;
      back_id /= 2;
      left_id /= 2;
      right_id /= 2;
      top_id /= 2;
      bottom_id /= 2;
      block_id /= 2;
      i -= j;
      j /= 8;
    }

  int tmp = 2;

  // calcul du facteur multiplicateur pour trouver la position
  while (j > 1)
    {
      tmp *= 2;
      j /= 8;
    }

  i += block_id & tmp;
  // recuperation de la liste et ajout de l'objet a l'interieur
  List<Object*>* ptr = static_tab_[i].obj_list;

  // on ajoute l'objet en tete de liste
  ptr = new List<Object*>;
  ptr->elt = obj;
  ptr->next = static_tab_[i].obj_list;
  static_tab_[i].obj_list = ptr;
}

bool
NodeRts::delete_obj (Object* obj)
{
  Vector3f pos = obj->pos_get ();
  pos %= 256; // recadrer dans le carre 256 * 256 * 256
  Vector3f size = obj->size_get ();

  Vector3d block_id (((int) pos (0)) / 16,
		     ((int) pos (1)) / 16,
		     ((int) pos (2)) / 16); // id du block en 3D

  Vector3d front_id (((int) pos (0) + (int) size (0)) / 16,
		     ((int) pos (1)) / 16,
		     ((int) pos (2)) / 16); // id du block en 3D

  Vector3d back_id (((int) pos (0) - (int) size (0)) / 16,
		    ((int) pos (1)) / 16,
		    ((int) pos (2)) / 16); // id du block en 3D

  Vector3d left_id (((int) pos (0)) / 16,
		    ((int) pos (1) - (int) size (1)) / 16,
		    ((int) pos (2)) / 16); // id du block en 3D

  Vector3d right_id (((int) pos (0)) / 16,
		     ((int) pos (1) + (int) size (1)) / 16,
		     ((int) pos (2)) / 16); // id du block en 3D

  Vector3d top_id (((int) pos (0)) / 16,
		   ((int) pos (1)) / 16,
		   ((int) pos (2) + (int) size (2)) / 16); // id du block en 3D

  Vector3d bottom_id (((int) pos (0)) / 16,
		      ((int) pos (1)) / 16,
		      ((int) pos (2) - (int) size (2)) / 16); // id du block en 3D

  int i = INDEX_16;
  int j = i - INDEX_32;

  // tant que je ne suis pas contenu entierement dans un block on remonte
  while (front_id != block_id ||
	 back_id != block_id ||
	 right_id != block_id ||
	 left_id != block_id ||
	 top_id != block_id ||
	 bottom_id != block_id)
    {
      front_id /= 2;
      back_id /= 2;
      left_id /= 2;
      right_id /= 2;
      top_id /= 2;
      bottom_id /= 2;
      block_id /= 2;
      i -= j;
      j /= 8;
    }

  int tmp = 2;
  // je recupere le multiplicateur de la position du noeud
  while (j > 1)
    {
      tmp *= 2;
      j /= 8;
    }

  i += block_id & tmp;
 
 // on recupere la liste et on detruit l'objet a l'interieur
  List<Object*>* ptr = static_tab_[i].obj_list;
  List<Object*>* pred = ptr;

  if (ptr == 0) // la liste est vide suppression impossible
    return false;
  else // elle contient des elements
    {
      while (ptr->elt != obj && ptr->next != 0)
	{
	  pred = ptr;
	  ptr = ptr->next;
	}

      if (ptr->elt == obj) // on a trouve l'element a supprimer
	{
	  if (ptr == static_tab_[i].obj_list)
	    static_tab_[i].obj_list = ptr->next;
	  else
	    pred->next = ptr->next;
	  delete ptr; // suppression reussie
	  return true;
	}
      return false; // il n'est pas dans la liste, suppression impossible
    }
}

void
NodeRts::accept (PhysicFunctor* phy)
{
  Object* obj = phy->object_get ();
  // on parcours l'octree et on appelle treat a chaque noeud
  Vector3f pos = obj->pos_get ();
  pos %= 256;
  Vector3f size = obj->size_get ();

  int nb_node = 16;

  // on recupere les 8 angles de l'objet
  int x1 = ((int) (pos (0) - size (0))) / 16;
  int y1 = ((int) (pos (1) - size (1))) / 16;
  int z1 = ((int) (pos (2) - size (2))) / 16;
  int x2 = ((int) (pos (0) + size (0))) / 16;
  int y2 = ((int) (pos (0) + size (0))) / 16;
  int z2 = ((int) (pos (2) + size (2))) / 16;

  int current = INDEX_16;

  // on traite tous les noeuds de taille 16 qui contiennent au moins un bout de l'objet
  for (int k = z1; k <= z2; ++k)
    for (int j = y1; j <= y2; ++j)
      for (int i = x1; i <= x2; ++i)
	(*phy) (static_tab_[current + i
			    + j * nb_node + k * nb_node * nb_node].obj_list);

  current = INDEX_32;
  int cpt = current - INDEX_64;

  while (current != 0)
    {
      nb_node /= 2;
      x1 /= 2;
      x2 /= 2;
      y1 /= 2;
      y2 /= 2;
      z1 /= 2;
      z2 /= 2;

      for (int k = z1; k <= z2; ++k)
	for (int j = y1; j <= y2; ++j)
	  for (int i = x1; i <= x2; ++i)
	    (*phy) (static_tab_[current + i
				+ j * nb_node + k * nb_node * nb_node].obj_list);

      current -= cpt;
      cpt /= 8;
    }

  (*phy) (static_tab_[0].obj_list);
}

void
NodeRts::accept (DisplayFunctor* disp)
{
  for (int i = 0; i < INDEX_8; ++i)
    (*disp) (static_tab_[i].obj_list);
}

void
NodeRts::accept (MapFunctor* mf)
{
  Vector3d tmp_pos;

  for (int z = 0; z < 32; ++z)
    for (int y = 0; y < 32; ++y)
      {
	for (int x = 0; x < 32; ++x)
	  {
	    tmp_pos.val_set (x, y, z);
	    send_node_8 (tmp_pos, mf);
	    (*mf) ();
	  }
      }

  // // positions 3d
  // Vector3d pos_8;
  // Vector3d global_8;
  // Vector3d pos_2;
  // Vector3d global_2;
  // Vector3d pos_1;
  // Vector3d global_1;
  // int i_tmp = 0;

  // // parcours du noeud de 8
  // dynamic_node* node = 0;
  // dyn_value* d = 0;
  // unsigned char pos;

  // // parcours du noeud de 2
  // unsigned char surface;

  // // pour tous les fils de 8
  // for (int i = INDEX_8; i < NB_NODE_256_TO_8; ++i)
  //   {
  //     // on calcule la position 3D du noeud courant
  //     pos_8.val_set (i_tmp % 32,
  // 		     (i_tmp / 32) % 32,
  // 		     i_tmp / (32 * 32));

  //     global_8.val_set (offset_i + pos_8 (0) * 8,
  // 			offset_j + pos_8 (1) * 8,
  // 			pos_8 (2) * 8);

  //     d = (dyn_value*) static_tab_[i].obj_list;

  //     // pour tous les noeud de 2 etant surfaces
  //     for (List<dynamic_node*>* tmp = d->obj_list; tmp != 0; tmp = tmp->next)
  // 	{
  // 	  // on recupere la position du noeud par rapport a son pere
  // 	  node = tmp->elt;
  // 	  pos = (unsigned char) node->position;

  // 	  // on calcule la position 3D du noeud de 2
  // 	  pos_2.val_set ((pos & (128 + 64)) / 64,
  // 			 (pos & (32 + 16)) / 16,
  // 			 (pos & (8 + 4)) / 4);
  // 	  global_2.val_set (global_8 (0) + pos_2 (0) * 2,
  // 			    global_8 (1) + pos_2 (1) * 2,
  // 			    global_8 (2) + pos_2 (2) * 2);

  // 	  surface = (unsigned char) node->is_surface;

  // 	  // on regarde chaque node de 1 dans le node de 2
  // 	  for (int j = 0; j < 8; ++j)
  // 	    if (surface & (1 << j)) // si le noeud de 1 est une surface
  // 	      {
  // 		// on recupere sa position
  // 		pos_1.val_set (j % 2,
  // 			       (j / 2) % 2,
  // 			       j / 4);
  // 		global_1.val_set (global_2 (0) + pos_1 (0),
  // 				  global_2 (1) + pos_1 (1),
  // 				  global_2 (2) + pos_1 (2));

  // 		// et on la file au foncteur
  // 		(*mf) (global_1);
  // 	      }
  // 	}
  //     ++i_tmp;
  //   }
}

void
NodeRts::insert_map (Map* map, int offset_i, int offset_j)
{
  Vector3d pos_8;
  Vector3d pos_2;
  List<dynamic_node*>* tmp = 0;
  dyn_value* d_tmp = 0;
  dynamic_node* curr = 0;
  unsigned char val;

  Vector3d* vect = 0;

  std::list<Vector3d*>* to_add = new std::list<Vector3d*>;
  std::list<Vector3d*>::iterator it;

  // on genere le points de map
  for (int i = offset_i; i < offset_i + 256; ++i)
    for (int j = offset_j; j < offset_j + 256; ++j)
      to_add->push_back (new Vector3d (i, j, (*map) (i, j)));

  // on y ajoute les points generes
  std::list<Vector3d*>* tmp_list = map->points_calc (offset_i, offset_j);
  to_add->splice (to_add->begin (), (*tmp_list));

  // pour tous les noeuds de 1 de la map dans laquelle je me trouve
  for (it = to_add->begin (); it != to_add->end (); ++it)
      {
	vect = *it;

	pos_8 (0, (*vect) (0) / 8); // quel fils de 8 je suis
	pos_8 (1, (*vect) (1) / 8);
	pos_8 (2, (*vect) (2) / 8);

	pos_8 %= 32;

	// quel est ma liste de noeuds de 2
	d_tmp = (dyn_value*) static_tab_[INDEX_8 + (pos_8 & 32)].obj_list;
	tmp = d_tmp->obj_list;

	pos_2 (0, ((*vect) (0) % 8) / 2); // quel noeud de 2 je suis
	pos_2 (1, ((*vect) (1) % 8) / 2);
	pos_2 (2, ((*vect) (2) % 8) / 2);

	// ma position par rapport au noeud de 8 pere
	val = pos_2 (0) * 64
	  + pos_2 (1) * 16
	  + pos_2 (2) * 4;

	// ma pos a l'interieur du noeud de 2
	pos_2 (0, (*vect) (0) % 2);
	pos_2 (1, (*vect) (1) % 2);
	pos_2 (2, (*vect) (2) % 2);

	// pas de noeud de 2 dans ce noeud de 8
	if (tmp == 0)
	  {
	    tmp = new List<dynamic_node*>;

	    curr = new dynamic_node;

	    curr->position = val;

	    curr->is_surface = (1 << (pos_2 & 2));
	    val = 0;

	    // on set full_empty pour tous les fils du nouveau node de 2
	    for (int cpt1 = 0, x = -pos_2 (0); cpt1 < 2; ++cpt1, ++x)
	      for (int cpt2 = 0, y = -pos_2 (1); cpt2 < 2; ++cpt2, ++y)
		for (int cpt3 = 0, z = -pos_2 (2); cpt3 < 2; ++cpt3, ++z)
		  if ((*map) ((*vect) (0) + x, (*vect) (1) + y) > (*vect) (2) + z)
		    val |= (1 << (cpt1 + cpt2 * 2 + cpt3 * 4));

	    curr->is_full_empty = val;

	    tmp->next = 0;
	    tmp->elt = curr;

	    d_tmp->obj_list = tmp;
	  }
	else
	  {
	    curr = tmp->elt;
	    while (tmp->next != 0 && curr->position != val)
	      {
		tmp = tmp->next;
		curr = tmp->elt;
	      }

	    // noeud de 2 existe deja
	    if (curr->position == val)
	      curr->is_surface |= (1 << (pos_2 & 2));
	    else // ou je le cree
	      {
		tmp->next = new List<dynamic_node*>;
		tmp = tmp->next;

		curr = new dynamic_node;

		curr->position = val;

		curr->is_surface = (1 << (pos_2 & 2));

		val = 0;

		// on set full_empty pour tous les fils du nouveau node de 2
		for (int cpt1 = 0, x = -pos_2 (0); cpt1 < 2; ++cpt1, ++x)
		  for (int cpt2 = 0, y = -pos_2 (1); cpt2 < 2; ++cpt2, ++y)
		    for (int cpt3 = 0, z = -pos_2 (2); cpt3 < 2; ++cpt3, ++z)
		      if ((*map) ((*vect) (0) + x, (*vect) (1) + y) > (*vect) (2) + z)
			val |= (1 << (cpt1 + cpt2 * 2 + cpt3 * 4));

		curr->is_full_empty = val;

		tmp->next = 0;
		tmp->elt = curr;
	      }
	  }

	// on set le bit du noeud de surface courant pour pouvoir recuperer
	// toutes les surfaces rapidement dans merge_surfaces
	d_tmp->is_full_empty[((*vect) (1) % 8) / 4 + (((*vect) (2) % 8) / 2) * 2] |=
	  (1 << ((((*vect) (0) % 8) / 2) + ((((*vect) (1) % 8) / 2) % 2) * 4));
      }

  for (it = to_add->begin (); it != to_add->end (); ++it)
    delete (*it);

  to_add->clear ();
  delete to_add;

  merge_surfaces (map, offset_i, offset_j);
  merge_up ();
}

void
NodeRts::merge_surfaces (Map* map, int offset_i, int offset_j)
{

  dyn_value* d = 0;
  char val;
  Vector3d pos_8;
  Vector3d global_8;
  Vector3d pos_2;
  Vector3d global_2;
  int i_tmp = 0;

  // pour tous les nodes de taille 8
  for (int i = INDEX_8; i < NB_NODE_256_TO_8; ++i)
    {
      // on calcul la pos du noeud courant
      pos_8.val_set (i_tmp % 32, (i_tmp / 32) % 32, i_tmp / (32 * 32));
      global_8.val_set (pos_8 (0) * 8 + offset_i,
			pos_8 (1) * 8 + offset_j,
			pos_8 (2) * 8);

      // on recupere les valeurs full/empty des fils
      d = (dyn_value*) static_tab_[i].obj_list;

      // parcours du tableau de full/empty
      for (int j = 0; j < 8; ++j)
	{
	  val = d->is_full_empty[j];
	  // parcours des valeurs des fils
	  for (int k = 0; k < 8; ++k)
	    {
	      // si il ne contient pas de surface ...
	      if (!(val & (1 << k)))
		{
		  pos_2.val_set (k % 4, (k / 4) + (j % 2) * 2, j / 2);
		  global_2.val_set (global_8 (0) + pos_2 (0) * 2,
				    global_8 (1) + pos_2 (1) * 2,
				    global_8 (2) + pos_2 (2) * 2);

		  // ... et qu'il est sous le sol de la map
		  if ((*map) (global_2 (0), global_2 (1)) > global_2 (2))
		    val |= (1 << k);
		}
	      else // si il contient au moins une surface
		val &= (255 - (1 << k));
	    }
	  // on set le resultat
	  d->is_full_empty[j] = val;
	}

      bool surface = false;
      bool full = false;

      // si je contiens des surfaces
      if (d->obj_list)
	surface = true;
      else // ou que je suis full
	{
	  for (int j = 0; j < 8 && !full; ++j)
	    if (((unsigned char) d->is_full_empty[j]) != 255)
	      full = true;

	  full = !full;
	}

      static_node_obj node;

      if (full || surface)
	{
	  pos_2 = pos_8;
	  pos_2 %= 2; // calcul de ma position par rapport a mon pere
	  pos_8 /= 2; // calcul du noeud pere

	  // recuperation de la liste de noeud du pere
	  node = static_tab_[INDEX_16 + (pos_8 & 16)];

	  // on set le bit a 1 du pere pour dire que je suis surface ou full
	  if (full)
	    node.is_full_empty |= (1 << (pos_2 & 2));
	  else
	    node.is_surface |= (1 << (pos_2 & 2));
	}
      ++i_tmp;
    }
}

void
NodeRts::merge_up ()
{
  char full_empty;
  char surface;

  Vector3d pos_node;
  Vector3d pos_son;

  int i_tmp = 0;

  int end = INDEX_8;
  int curr = INDEX_16;
  int prev = INDEX_32;

  // nombre de noeud par dimension pour un etage donne
  int nb_node = 8;

  static_node_obj node;

  // jusqu'au noeud de 256
  while (curr != 0)
    {
      i_tmp = 0;
      // on parcours tous les noeuds de l'etage courant
      for (int i = curr; i < end; ++i)
	{
	  // on recupere la position du noeud
	  pos_node.val_set (i_tmp % nb_node,
			    (i_tmp / nb_node) % nb_node,
			    i_tmp / (nb_node * nb_node));

	  // on recupere le full/empty et surface du noeud courrant
	  full_empty = static_tab_[curr + (pos_node & nb_node)].is_full_empty;
	  surface = static_tab_[curr + (pos_node & nb_node)].is_surface;

	  if (surface || full_empty)
	    {
	      pos_son = pos_node;
	      pos_son %= 2; // calcul de ma position par rapport a mon pere
	      pos_node /= 2; // calcul du noeud pere

	      // recuperation de la liste de noeud du pere
	      node = static_tab_[curr + (pos_node & nb_node)];

	      // on set le bit a 1 du pere pour dire que je suis surface ou full
	      if (full_empty)
		node.is_full_empty |= (1 << (pos_son & 2));
	      else
		node.is_surface |= (1 << (pos_son & 2));
	    }
	  ++i_tmp;
	}

      // on remonte d'un etage
      end = curr;
      curr = prev;
      prev = prev - ((end - curr) / 8);
      nb_node /= 2;
    }
}

void
NodeRts::send_node_8 (Vector3d pos, MapFunctor* mf)
{
  // on recupere le noeud de 8 dans le tableau
  static_node_obj node_8 = static_tab_[INDEX_8 + (pos & 32)];

  dyn_value* data_8 = (dyn_value*) node_8.obj_list;
  List<dynamic_node*>* node_2 = data_8->obj_list;

  // et on envoi tous ses fils de taille 2
  unsigned char* to_send = 0;

  while (node_2)
    {
      to_send = new unsigned char[3];

      to_send[0] = node_2->elt->is_surface;
      to_send[1] = node_2->elt->position;
      to_send[2] = '\0';

      (*mf) (to_send);
      node_2 = node_2->next;
    }
}

void
NodeRts::accept (GetObjectFunctor* objf, Vector2d* pos)
{
  Vector3d block_init;
  int nb_blocks = 8;

  // on recupere le premier node de 16 des 2 blocks de 128
  block_init.val_set ((*pos) (0) * 8, (*pos) (1) * 8, 0);

  Vector3d parc;
  int cur_index = INDEX_16;
  int to_sub = INDEX_16 - INDEX_32;

  // on envoi les etages de 16, 32, 64, 128
  for (int step = 0; step < 4; ++step)
    {
      parc = block_init;

      // on traite tous les blocks de la taille courante
      for (int x = 0; x < nb_blocks; ++x)
	{
	  parc (1, block_init (1));
	  for (int y = 0; y < nb_blocks; ++y)
	    {
	      parc (2, block_init (2));
	      for (int z = 0; z < 2 * nb_blocks; ++z)
		{
		  (*objf) (static_tab_[cur_index + (parc & (nb_blocks * 2))].obj_list);
		  parc (2, parc (2) + 1);
		}
	      parc (1, parc (1) + 1);
	    }
	  parc (0, parc (0) + 1);
	}

      // et on remonte d'un etage
      nb_blocks /= 2;
      block_init /= 2;
      cur_index -= to_sub;
      to_sub /= 8;
    }
}
